/*
 * File - ros_sensor.cpp
 *
 * This file is part of the Inuitive SDK
 *
 * Copyright (C) 2014-2020 Inuitive All Rights Reserved
 *
 */

/**
 * \cond INTERNAL
 */

#include "config.h"

#include <limits>
#include<mutex>

#include <nodelet/nodelet.h>
#include <ros/rate.h>


/**
 * \endcond
 */

#include "ros_publisher.h"
#include "ros_sensor.h"
#include "ros_imu_publisher.h"
#include "ros_video_publisher.h"
#include "ros_webcam_publisher.h"
#include "ros_disparity_publisher.h"
#include "ros_pointcloud_publisher.h"
#include "ros_depth_publisher.h"
#include "ros_fisheye_publisher.h"
//#include "ros_slam_publisher.h"
//#include "ros_objectdetection_publisher.h"
#include "ros_features_publisher.h"
/**
 * \file ros_sensor.cpp
 *
 * \brief CRosSensor
 */

/**
 * \cond INTERNAL
 */
using namespace std;

static const char rcsid[]="$Id: fc782d698a5cc132f43214c275b8892ba6b49a97 $";
static const int connectionTimeout=10000;
static bool sensorconnectionok = false;

#define myABS(X,Y) (X >= Y) ? (X -Y) : (Y - X)
/**
 * \endcond
 */

namespace __INUROS__NAMESPACE__
{
    CRosSensor::CRosSensor(ros::NodeHandle& _node, int fps, const std::string _str)
        : CRosPublisherBookkeeping(_str)
        , mLastKnownTemperature(std::numeric_limits<int>::max())
        , node(_node)
        , mTimestampWeb(0)
    {
        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());

        image_transport = std::make_shared<image_transport::ImageTransport>(node);

        while(sensorconnectionok == false)
        {
            // InuDev::CInuError err = StartStream();
            bool finnished = ( StartStream() != InuDev::EErrorCode::eOK || !StartTemperaStream() );
            StopTemperaStream();

            if (finnished)
            {
                //throw std::logic_error("CRosSensor");
                //  NODELET_WARN_STREAM("*** ROSSensor Init failed ***: " );
                sensorconnectionok = false;
                ros::Duration(2).sleep();
                if(sensor)
                {
                    StopStream();
                }
            }
            else
            {
                sensorconnectionok = true;
                state = sensor->GetState();
                //   NODELET_WARN_STREAM("*** ROSSensor Init succeed ***: " );
                connectionStateTimer = node.createTimer(ros::Duration(4), &CRosSensor::connectionCheckCallback, this);
            }
        }

        std::string serialNumber = getSerialNumber();

        #if 1    
        if (publisherList.empty())
        {
            NODELET_WARN_STREAM("Using default topic names. If needed, use topic remapping to change topic name.");
            publisherList.push_back(std::make_shared<CRosWebcamPublisher>(node, image_transport, this));
            publisherList.push_back(std::make_shared<CRosVideoPublisher>(node, image_transport, this));
            publisherList.push_back(std::make_shared<CRosDepthPublisher>(node, image_transport, this));
            publisherList.push_back(std::make_shared<CRosDisparityPublisher>(node, image_transport, this));
            publisherList.push_back(std::make_shared<CRosPointcloudPublisher>(node, image_transport, this));
            publisherList.push_back(std::make_shared<CRosFisheyePublisher>(node, image_transport, this));
            // publisherList.push_back(std::make_shared<CRosSlamPublisher>(node, image_transport, this));
            publisherList.push_back(std::make_shared<CRosImuPublisher>(node, nullptr, this));
            // publisherList.push_back(std::make_shared<CRosObjectDetectionPublisher>(node, nullptr, this));
            publisherList.push_back(std::make_shared<CRosFeaturesPublisher>(node, nullptr, this));
        }
        #else
        if (publisherList.empty())
        {
            NODELET_WARN_STREAM("Using default topic names. If needed, use topic remapping to change topic name.");
            publisherList.push_back(std::make_shared<CRosWebcamPublisher>(node, image_transport, this,"camera/color/image_raw/"+ serialNumber));
            publisherList.push_back(std::make_shared<CRosVideoPublisher>(node, image_transport, this,"sensor_msgs/Image/Video/right/image/"+ serialNumber ,"sensor_msgs/Image/Video/left/image/"+ serialNumber));
            publisherList.push_back(std::make_shared<CRosDepthPublisher>(node, image_transport, this,"camera/aligned_depth_to_color/image_raw/"+ serialNumber));
            publisherList.push_back(std::make_shared<CRosDisparityPublisher>(node, image_transport, this,"sensor_msgs/Image/Video/disparity/"+ serialNumber));
            publisherList.push_back(std::make_shared<CRosPointcloudPublisher>(node, image_transport, this,"sensor/PointCloud2/"+ serialNumber));
            publisherList.push_back(std::make_shared<CRosFisheyePublisher>(node, image_transport, this,"sensor_msgs/Image/Fisheye/"+ serialNumber));
            // publisherList.push_back(std::make_shared<CRosSlamPublisher>(node, image_transport, this,"sensor_msgs/Path/"+ serialNumber));
            publisherList.push_back(std::make_shared<CRosImuPublisher>(node, nullptr, this,"sensor_msgs/Imu/"+ serialNumber));
            // publisherList.push_back(std::make_shared<CRosObjectDetectionPublisher>(node, nullptr, this,"vision_msgs/Detections/"+ serialNumber));
            publisherList.push_back(std::make_shared<CRosFeaturesPublisher>(node, nullptr, this,"camera/fisheye/features/"+ serialNumber));
        }

        #endif

        timer = node.createTimer(ros::Duration(1.0), &CRosSensor::timerCallback, this);
    }

    std::shared_ptr<InuDev::CInuSensorExt> CRosSensor::getSensor()
    {
        return sensor;
    }

    std::string CRosSensor::getSerialNumber()
    {
        return version[InuDev::CEntityVersion::EEntitiesID::eSerialNumber].VersionName;
    }

    void CRosSensor::InsertWebImage(const cv::Mat &img, uint64_t ts)
    {
        unique_lock<mutex> lock(mMutexWeb);
        mMatWeb = img.clone();
        mTimestampWeb = ts;
    }

    uint64_t CRosSensor::GetWebImage(cv::Mat &img, uint64_t ts)
    {
        unique_lock<mutex> lock(mMutexWeb);
        if (mMatWeb.empty())
        {
         //   NODELET_INFO_STREAM(" NO WEB IMAGE: " << ts );
            return 0;
        }
        uint64_t dif = myABS(ts, mTimestampWeb);
        if (dif > 1e9)
        {
            //NODELET_INFO_STREAM("melodic: " << ts << "," << mTimestampWeb << "," << dif);
            return 0;
        }
        img = mMatWeb.clone();
        return mTimestampWeb;
    }
   

    void CRosSensor::connectionCheckCallback(const ros::TimerEvent& e)
    {
       if(sensor)
        {
            InuDev::CInuSensor::ESensorState newState = sensor->GetState();
//          NODELET_WARN_STREAM(__INUROS_FUNCTION_NAME__ << ": InuSensor  state  ****** new state =  " << int(newState));
 
            if ( !mCheckTemperaSucceeded || int(newState) == 0 || int(newState) == 4 || int(newState) == 3)
            {
                RestartSensor(true);
            }

            state = newState;

        }
    }
    void CRosSensor::RestartSensor(bool fromThread)   //restart sensor
    {
        std::unique_lock<std::mutex> my_unique(mMutexOpt, std::try_to_lock);

        if(!my_unique.owns_lock())
        {
            return;
        }
        if (!sensor)
        {
            return;
        }
        my_unique.unlock();

        // Log restart sensor
        NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << " ============= Stop Sensor ==================\n");
        InuDev::CInuError err=StopStream();
        if(err==InuDev::EErrorCode::eOK)
        {
            NODELET_WARN_STREAM(" ========sensor stop success=========== ");
        } 
        
        ros::Duration(2).sleep();

        // Log Start sensor
        NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << " ============= Start Sensor ==================\n"); 
        
        InuDev::CInuError error=StartStream();
        if(error==InuDev::EErrorCode::eOK)
        {
            NODELET_WARN_STREAM(" ========sensor restart success=========== ");
        }

        if(StartTemperaStream())
        {
            NODELET_WARN_STREAM(" ========check tempera success=========== ");
        }

        StopTemperaStream();

        NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << " ============= Publish Recovery ==================\n");
        for (auto const& it : publisherList)
        {
            it->UpdatePublisherRecover();
        }
    }


    InuDev::CInuError CRosSensor::StartStream()
    {
        std::unique_lock<std::mutex>  uniqueLock(mMutexOpt);

        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());

        if (sensor)
        {
            // Already created (in constructor)
            return InuDev::EErrorCode::eOK;
        }

        node.param<std::string>("IP_Address", mIpAddress, "");
        node.param<std::string>("Device_ID", mServiceID, "");

        if (mIpAddress != "")
        {
            NODELET_WARN_STREAM(" Starting default sensor : " << mIpAddress);
            sensor = InuDev::CInuSensorExt::Create(mServiceID, mIpAddress);
        }
        else if (mServiceID != "")
        {
            NODELET_WARN_STREAM(" Starting sensor with ID: " << mServiceID);
            sensor = InuDev::CInuSensorExt::Create(mServiceID);
        }
        else
        {
            NODELET_WARN_STREAM(" Starting default sensor : " << mServiceID);
            sensor = InuDev::CInuSensorExt::Create();
        }

        if (!sensor)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": sensor is null!!");
            return InuDev::EErrorCode::eInitError;
        }

        node.getParam("FPS", reinterpret_cast<int&>(mDeviceParams.FPS));
        node.getParam("Resolution", reinterpret_cast<int&>(mDeviceParams.SensorRes));

        NODELET_WARN_STREAM("Starting sensor with FPS: " << mDeviceParams.FPS << " Resolution: " << mDeviceParams.SensorRes);
        InuDev::CInuError err = InuDev::EErrorCode::eOK;

        if (mDeviceParams.SensorRes == 0 && mDeviceParams.FPS == -1)
        {
            err = sensor->Init(mHwInfo, mVecDpeParams);
        }
        else
        {
            err = sensor->Init(mHwInfo, mVecDpeParams, mDeviceParams);
        }
        if (mDeviceParams.FPS == -1)
        {
            mDeviceParams.FPS = std::numeric_limits<uint32_t>::max();
        }

        if (err != InuDev::EErrorCode::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": InuSensor failed to Init: 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
            return err;
        }

        mChannels = mHwInfo.GetChannels();

        // InuDev::CInuSensor::CallbackFunction callback = std::bind(&CRosSensor::SensorCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
        // err = sensor->Register(callback);

        // if (err != InuDev::EErrorCode::eOK)
        // {
        //     NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": InuSensor failed register Callback: 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
        //     return err;
        // }

        std::map<uint32_t, InuDev::CChannelControlParams> mapChannelParams;
        std::map<uint32_t, InuDev::CChannelSize> channelsSize;
        uint32_t videoChannel;
        bool foundVideoChannel = false;

        for (auto channel : mChannels)
        {
            if (channel.second.ChannelType == InuDev::eStereoChannel)
            {
                videoChannel = channel.first;
                foundVideoChannel = true;
            }

            if (channel.second.ChannelType == InuDev::eGeneralCameraChannel || channel.second.ChannelType == InuDev::eDepthChannel)
            {
                int channelResolution = 0;
                InuDev::CChannelControlParams channelParams;

                if (channel.second.ChannelType == InuDev::eGeneralCameraChannel)
                {
                    node.getParam("RGBResolution", channelResolution);
                    channelParams = InuDev::CChannelControlParams(InuDev::ESensorResolution(channelResolution), mDeviceParams.FPS);
                }
                else
                {
                    node.getParam("DepthResolution", channelResolution);
                    channelParams = InuDev::CChannelControlParams(InuDev::ESensorResolution(channelResolution), mDeviceParams.FPS);
                    #ifdef _GE428_
                    int _D2Cchannel = 0;
                    node.getParam("DepthRegistarionChannel",_D2Cchannel);

                    if(_D2Cchannel != -1){
                        channelParams.ActivateRegisteredDepth = true;
                        channelParams.RegisteredDepthChannelID = _D2Cchannel;
                    }
                    #endif
                }

                mapChannelParams.insert(std::make_pair(channel.first, channelParams));
                NODELET_DEBUG_STREAM("Changing resolution for channel: " << std::to_string(channel.first) << ", to: " << channelResolution);
            }
            else if (channel.second.ChannelType == InuDev::eTrackingChannel)
            {
                //mapChannelParams.insert(std::make_pair(channel.first, InuDev::CChannelControlParams(InuDev::eFull, mDeviceParams.FPS)));
                InuDev::CChannelControlParams channelParams;
                channelParams.InterleaveMode = InuDev::eInterleave;
                channelParams.SensorRes = InuDev::eFull;
                channelParams.FPS = mDeviceParams.FPS;
                mapChannelParams.insert(std::make_pair(channel.first, channelParams));
            }
        }

        if (foundVideoChannel)
        {
            InuDev::CChannelControlParams channelParams;
            channelParams.InterleaveMode = InuDev::eInterleave;
            channelParams.SensorRes = mDeviceParams.SensorRes;
            channelParams.FPS = mDeviceParams.FPS;
            mapChannelParams.insert(std::make_pair(videoChannel, channelParams));

            err = sensor->Start(channelsSize, mapChannelParams);
        }
        else
        {
            err = sensor->Start();
        }

        if (err != InuDev::EErrorCode::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": InuSensor failed to Start: 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
            return err;
        }

        err = sensor->GetVersion(version);

        if (err != InuDev::EErrorCode::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": Failed to get Sensor version: 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
            return err;
        }

        auto it = version.find(InuDev::CEntityVersion::EEntitiesID::eSerialNumber);

        if (it == version.end())
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": Failed to get Sensor's serial number: 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
            return InuDev::EErrorCode::eInternalError;
        }

        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": Serial number: " << it->second.VersionName);

        calibrationVersion = version.find(InuDev::CEntityVersion::EEntitiesID::eCalibrationVersion)->second.VersionName;
        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": calibrationVersion : " << calibrationVersion);

        auto calibrationData = std::make_shared<InuDev::CCalibrationData>();

        err = sensor->GetCalibrationData(*calibrationData.get());

        if (err != InuDev::EErrorCode::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": Failed to retreive calibration data: 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
            return err;
        }

        mCalibrationData = calibrationData; // shared_ptr assignemt

        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": Started " << getName());

        return InuDev::EErrorCode::eOK;
    }

    InuDev::CInuError CRosSensor::StopStream()
    {
        std::unique_lock<std::mutex>  uniqueLock(mMutexOpt);

        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());

        if (!sensor)
        {
            return InuDev::EErrorCode::eOK;
        }

        //publisherList.clear();

        InuDev::CInuError err = sensor->Register(nullptr);

        if (err != InuDev::EErrorCode::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": Unregister: 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
        }

        err = sensor->Stop();

        if (err != InuDev::EErrorCode::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": Stop: 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
        }

        err = sensor->Terminate();

        if (err != InuDev::EErrorCode::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": Terminate: 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
        }

        sensor = nullptr;

        return err;
    }

    void CRosSensor::MiddleUpdate()
    {
        // start/stop individual streams

        for (auto const& it : publisherList)
        {
            it->UpdatePublisher();
        }
    }

    CRosSensor::~CRosSensor()
    {
        publisherList.clear();

        StopStream();
    }

    InuDev::CInuError CRosSensor::SensorCallback(std::shared_ptr<InuDev::CInuSensor> iStream, InuDev::EConnectionState iConnectionState, InuDev::CInuError iError)
    {
        NODELET_DEBUG_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());

        if (iError == InuDev::EErrorCode::eOK)
        {
            NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": connection state = " << iConnectionState);
        }
        else
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": connection state = " << iConnectionState << ", error = 0x" << std::hex << std::setw(8) << std::setfill('0') << int(iError));
        }

        switch (int(iError))
        {
        case InuDev::EErrorCodeExt::eUSBDisconnect:

            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": Target disconnected");
            return InuDev::EErrorCode::eOK;
            break;

        case InuDev::EErrorCodeExt::eUSBConnect:

            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": Target re-connected");
            return InuDev::EErrorCode::eOK;
            break;
        }
    }

    int CRosSensor::GetNumSubscribers()
    {
        int n = 0;
        for (auto const& it : publisherList)
        {
            n += it->GetNumSubscribers();
        }

        publisherPrev = publisherCurr;
        publisherCurr = n;

        return publisherCurr;
    }

    void CRosSensor::timerCallback(const ros::TimerEvent& e)
    {
        GetNumSubscribers();

        UpdatePublisher();

        if (!mLastKnownTemperature.compare_exchange_strong(mCurrentTemperature, mCurrentTemperature))
        {
            NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": new sensor temperature: " << mLastKnownTemperature);
            NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": Getting Calibration Data for " << getName());

            auto calibrationData = std::make_shared<InuDev::CCalibrationData>();
            InuDev::EErrorCode err = sensor->GetCalibrationData(*calibrationData.get());

            if (err == InuDev::EErrorCode::eOK)
            {
                mCalibrationData = calibrationData; // shared_ptr assigment
            }
            else
            {
                NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": Failed to retreive calibration data: 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
            }
        }
    }

    std::vector<int> CRosSensor::GetSensors(uint32_t channel)
    {
        std::vector<int> sensors;

        sensors.clear();

        if (mChannels.find(channel) != mChannels.end())
        {

            auto hwc = mChannels[channel].ChannelSensorsID;

            for (auto& elem : hwc)
            {
                sensors.push_back(int(elem));
            }
        }

        return sensors;
    }

    std::shared_ptr<InuDev::CCalibrationData> CRosSensor::GetCalibrationData(int currTemp, int iChannelID)
    {
        mCurrentTemperature = currTemp;

        auto calibrationData = std::make_shared<InuDev::CCalibrationData>();
        sensor->GetCalibrationData(*calibrationData.get(), iChannelID, mCurrentTemperature);

        mCalibrationData = calibrationData; // shared_ptr assignemt

        return mCalibrationData;
    }

    void CRosSensor::TemperaCallback(std::shared_ptr<InuDev::CTemperaturesStream> iStream, std::shared_ptr<const InuDev::CTemperaturesFrame>  iFrame, InuDev::CInuError retCode)
    {
        if (retCode != InuDev::eOK || iFrame->Valid == false)
        {
            return;
        }

        float tempera[3];//sensor1,sensor2,pvt;

        iFrame->GetTemperature((InuDev::CTemperaturesFrame::ESensorTemperatureType)1,tempera[0]);
        iFrame->GetTemperature((InuDev::CTemperaturesFrame::ESensorTemperatureType)2,tempera[1]);
        iFrame->GetTemperature(InuDev::CTemperaturesFrame::ESensorTemperatureType::ePVT,tempera[2]);

        //NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ <<  "sensor1_tempera  = " <<tempera[0]<< " sensor2_tempera  = " <<tempera[1]<<" pvt_tempera  = " <<tempera[2]);
        NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ <<  "sensor1_tempera  = " <<tempera[0]<< " sensor2_tempera  = " <<tempera[1]<<" pvt_tempera  = " <<tempera[2]);

        std::unique_lock<std::mutex> lk(mMutexTempera);

        mCheckTemperaSucceeded=true;

        mConditionVariableTempera.notify_one();
    }

    
    bool CRosSensor::StartTemperaStream()
    {
        mCheckTemperaSucceeded=false;

        mTemperaStream = sensor->CreateTemperaturesStream(InuDev::CTemperaturesFrame::ESensorTemperatureType::eAll);
        if (mTemperaStream == nullptr)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": Unexpected error, failed to get Temperature Stream ");
            return false;
        }

        InuDev::EErrorCode retCode = std::static_pointer_cast<InuDev::CTemperaturesStream>(mTemperaStream)->Init();
        if (retCode != InuDev::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": Temperature failed to Init: 0x" << std::hex << std::setw(8) << std::setfill('0') << int(retCode));
            return false;
        }

        retCode = std::static_pointer_cast<InuDev::CTemperaturesStream>(mTemperaStream)->Start();
        if (retCode != InuDev::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": Temperature failed to Start: 0x" << std::hex << std::setw(8) << std::setfill('0') << int(retCode));
            return false;
        }

        // retCode = rgbStream->Register([this](auto stream, auto frame, auto error) { FrameCallback(stream, frame, error); });
        retCode = std::static_pointer_cast<InuDev::CTemperaturesStream>(mTemperaStream)->Register(std::bind(&CRosSensor::TemperaCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));

        if (retCode != InuDev::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": Temperature failed to Register: 0x" << std::hex << std::setw(8) << std::setfill('0') << int(retCode));
            return false;
        }

        std::unique_lock<std::mutex> lk(mMutexTempera);

        mConditionVariableTempera.wait_for(lk, std::chrono::seconds(2), [&]() {return mCheckTemperaSucceeded; });

        if (!mCheckTemperaSucceeded)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": Temperature failed to check because of timeout." );
            return false;
        }

        return true;
    }

    bool CRosSensor::StopTemperaStream()
    {
        if (!mTemperaStream) { return true; }

        InuDev::CInuError retCode = std::static_pointer_cast<InuDev::CTemperaturesStream>(mTemperaStream)->Register(nullptr);
        if (retCode != InuDev::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": Temperature failed to Unregister: 0x" << std::hex << std::setw(8) << std::setfill('0') << int(retCode));
            return false;
        }

        retCode = mTemperaStream->Stop();
        if (retCode != InuDev::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": Temperature failed to Stop: 0x" << std::hex << std::setw(8) << std::setfill('0') << int(retCode));
            return false;
        }
        std::cout << "Temperature acquisition stopped" << std::endl;

        retCode = mTemperaStream->Terminate();
        if (retCode != InuDev::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": Temperature failed to Terminate: 0x" << std::hex << std::setw(8) << std::setfill('0') << int(retCode));
            return false;
        }
        std::cout << "Temperature Stream was finalized" << std::endl;

        mTemperaStream = nullptr;
        
        return true;
    }    
}
